Self-Driving Car Engineer Nanodegree¶

Advanced Lane Finding Project By Abdelkoddous Khamsi¶

The goals / steps of this project are the following:

Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
Apply a distortion correction to raw images.
Use color transforms, gradients, etc., to create a thresholded binary image.
Apply a perspective transform to rectify binary image ("birds-eye view").
Detect lane pixels and fit to find the lane boundary.
Determine the curvature of the lane and vehicle position with respect to center.
Warp the detected lane boundaries back onto the original image.
Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.

First, I'll compute the camera calibration using chessboard images¶

import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import math

%matplotlib qt

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.

# Make a list of calibration images
images = glob.glob('camera_cal/calibration*.jpg')
N = len(images)

print(images)
print(type(images))
print("Number of images ==>", N)

# Step through the list and search for chessboard corners
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

    # Find the chessboard corners
    ret, corners = cv2.findChessboardCorners(gray, (9,6),None)

    # If found, add object points, image points
    if ret == True:
        objpoints.append(objp)
        imgpoints.append(corners)

        # Draw and display the corners
        img = cv2.drawChessboardCorners(img, (9,6), corners, ret)
        cv2.imshow('img',img)
        cv2.waitKey(1000)
    else:
        print("No corners found in ", fname)

cv2.destroyAllWindows()

['camera_cal\\calibration1.jpg', 'camera_cal\\calibration10.jpg', 'camera_cal\\calibration11.jpg', 'camera_cal\\calibration12.jpg', 'camera_cal\\calibration13.jpg', 'camera_cal\\calibration14.jpg', 'camera_cal\\calibration15.jpg', 'camera_cal\\calibration16.jpg', 'camera_cal\\calibration17.jpg', 'camera_cal\\calibration18.jpg', 'camera_cal\\calibration19.jpg', 'camera_cal\\calibration2.jpg', 'camera_cal\\calibration20.jpg', 'camera_cal\\calibration3.jpg', 'camera_cal\\calibration4.jpg', 'camera_cal\\calibration5.jpg', 'camera_cal\\calibration6.jpg', 'camera_cal\\calibration7.jpg', 'camera_cal\\calibration8.jpg', 'camera_cal\\calibration9.jpg']
<class 'list'>
Number of images ==> 20
No corners found in  camera_cal\calibration1.jpg
No corners found in  camera_cal\calibration4.jpg
No corners found in  camera_cal\calibration5.jpg

## Camera calibration given
%matplotlib inline

def cal_undistort(img, objpoints, imgpoints):
    # Use cv2.calibrateCamera() and cv2.undistort()
    cam_calib = cv2.calibrateCamera(objpoints, imgpoints, img.shape[:-1], None, None)
    undist = cv2.undistort(img, cam_calib[1], cam_calib[2])
    return undist

def PlotComparisonView(image_before, image_after, performedProcessing, colormap = (None, None)):
    """
    To avoid repeting ploting commands every time, will be useful throughout
    this notebook 
    """

    f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
    f.tight_layout()
    ax1.imshow(image_before, cmap = colormap[0])
    ax1.set_title('Original Image', fontsize=35)
    ax2.imshow(image_after, cmap = colormap[1])
    ax2.set_title(performedProcessing + ' Image', fontsize=35)
    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
    plt.show()

img = cv2.imread(images[1])

print("objpoints array len", len( objpoints))
print("imgpoints array len", len( imgpoints))

undistorted_img = cal_undistort(img, objpoints, imgpoints)

PlotComparisonView(img, undistorted_img, 'Undistorted')

objpoints array len 17
imgpoints array len 17

# Visualizing before and after of Undistortion on some chessboard images
for i in range(13,16):
    
    img = cv2.imread(images[i])
    undistorted_img = cal_undistort(img, objpoints, imgpoints)
    
    PlotComparisonView(img, undistorted_img, 'Undistorted')

Then, I'll Apply this distortion correction to raw road images¶

import matplotlib.image as mpimg

# Visualizing before and after of Undistortion on real world application image
test_images = glob.glob('test_images/test*.jpg')

for i in range(1,4):
    img = mpimg.imread(test_images[i])
    undistorted_img = cal_undistort(img, objpoints, imgpoints)
    PlotComparisonView(img, undistorted_img, 'Undistorted Road')

I'll use Sobel operator gradients and Color transforms to create a thresholded binary image¶

Let's start playing with the sobel operator

def axis_gradient(img, ksize=9, thresh=(30,255)):
    ## x gradient

    # convert to gray scale
    #if channel == 'gray':
    #    gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    #else:
    #    gray_image = img
    if len(img.shape) == 3:
        gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    elif len(img.shape) ==2:
        gray_image = img
    
    height = img.shape[0]
    width = img.shape[1]
    # derivative along x
    sobelx = cv2.Sobel(gray_image, cv2.CV_64F, 1, 0, ksize)

    abs_sobel = np.absolute(sobelx)

    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))

    single_axis_binary = np.zeros_like(gray_image, dtype=np.uint8)
    #single_axis_binary = np.zeros((height,width,1), np.uint8)

    single_axis_binary[ (scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1

    return single_axis_binary#cv2.Canny(gray_image, 20, 50) #

def magnitude_gradient(img, ksize=9, thresh=(50,255),channel='gray'):
    ## mag gradient
    # convert to gray scale
    #if channel == 'gray':
    #    gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    #else:
    #    gray_image = img
    if len(img.shape) == 3:
        gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    elif len(img.shape) ==2:
        gray_image = img
        
    # derivative along x and y
    sobelx = cv2.Sobel(gray_image, cv2.CV_64F, 1, 0, ksize)
    sobely = cv2.Sobel(gray_image, cv2.CV_64F, 0, 1, ksize)
    sobelxy = np.sqrt(sobelx**2 + sobely**2)

    scaled_sobel = np.uint8(255*sobelxy/np.max(sobelxy))

    mag_binary = np.zeros_like(gray_image)
    mag_binary[ (scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1

    return mag_binary

def direction_gradient(img, ksize=9, thresh=(0.7,1.3),channel='gray'):
    ## direction gradient

    # convert to gray scale
    #if channel == 'gray':
    #    gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    #else:
    #    gray_image = img
    if len(img.shape) == 3:
        gray_image = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    elif len(img.shape) ==2:
        gray_image = img
        
    # derivative along x and y
    sobelx = cv2.Sobel(gray_image, cv2.CV_64F, 1, 0, ksize)
    sobely = cv2.Sobel(gray_image, cv2.CV_64F, 0, 1, ksize)
    
    abs_sobelx = np.absolute(sobelx)
    abs_sobely = np.absolute(sobely)

    sobel_direction = np.arctan2(abs_sobely, abs_sobelx)

    dir_binary = np.zeros_like(sobel_direction)
    dir_binary[ (sobel_direction >= thresh[0]) & (sobel_direction <= thresh[1])] = 1

    return dir_binary

image = mpimg.imread(test_images[1])

# Apply all three distinct gradient
axis_binary_output = axis_gradient(image)
mag_binary_output = magnitude_gradient(image)
dir_binary_output = direction_gradient(image)


PlotComparisonView(image, axis_binary_output, 'Thresholded x-axis binary', (None, 'gray'))
PlotComparisonView(image, mag_binary_output, 'Thresholded Magnitude binary', (None, 'gray'))
PlotComparisonView(image, dir_binary_output, 'Thresholded Direction binary', (None, 'gray'))

# Play with threshold combination
combined_binary = np.zeros_like(axis_binary_output)
combined_binary[ (axis_binary_output==1) | (mag_binary_output*dir_binary_output == 1) ] = 1

PlotComparisonView(image, combined_binary, 'Combined Threshold binary', (None, 'gray'))

def combinedGradient_thresholdedBinary(img):
    
    #print("Image shape ==>",img.shape)
    axis_binary_output = axis_gradient(img)
    mag_binary_output = magnitude_gradient(img)
    dir_binary_output = direction_gradient(img)
    
    combined_binary = np.zeros_like(axis_binary_output)
    combined_binary[ (axis_binary_output==1) | (mag_binary_output*dir_binary_output == 1) ] = 1

    return combined_binary

# Check on all the test images
for i in range ( len(test_images) ):
    
    img = mpimg.imread(test_images[i])
    
    combined_threshold = combinedGradient_thresholdedBinary(img)
    
    PlotComparisonView(img, combined_threshold, 'Combined Thresholds ', (None, 'gray'))
    
    #cv2.imwrite("output_images/write_test"+str(i)+".jpg", combined_threshold)

Let's start playing with color spaces to solve the issues of diffirent colorations of th road we can see for example in the first image.

def to_HLS_Color_Channel(img, L_thresh=(200,255), S_thresh=(200,255)):
    
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    
    H_thresh = (10,40)
    H = hls[:,:,0]
    L = hls[:,:,1]
    S = hls[:,:,2]
    
    S_binary = np.zeros_like(S)
    L_binary = np.zeros_like(L)
    H_binary = np.zeros_like(H)

#    S_binary[ ( ( L >= L_thresh[0] ) & ( L <= L_thresh[1] ) ) 
#            | ( ( S >= S_thresh[0]) & (S <= S_thresh[1] ) ) ] = 1

    L_binary[ (L >= L_thresh[0]) & (L <= L_thresh[1]) ] = 1 # to extract white lanes
    S_binary[ (S >= S_thresh[0]) & (S <= S_thresh[1]) ] = 1
    H_binary[ (H >= H_thresh[0]) & (H <= H_thresh[1]) ] = 1
    
    return L_binary | S_binary

image = mpimg.imread(test_images[0])
#s_binary = to_S_Color_Channel(image,(190,255))
s_binary = to_HLS_Color_Channel(image)

PlotComparisonView(image, s_binary, 'HLS Channel ', (None, 'gray'))

# Check the color transform and the combined sobel gradients on all the test images
for i in range ( len(test_images) ):
    
    img = mpimg.imread(test_images[i])
    
    s_binary = to_HLS_Color_Channel(img)
    
    combined_threshold = combinedGradient_thresholdedBinary(s_binary)
    
    PlotComparisonView(img, combined_threshold, 'Combined Thresholds ', (None, 'gray'))

I'll use now a perspective transform to rectify binary image ("bird's eye view")¶

def drawConvexPolygonEdgesVertices(img, pts, edge = True, vertex = True):
    """
    pts: a sequence of pts that create a convex polygon
    """
    col = [255, 0, 0]
    thick = 3
    rad = 15
    
    img_copy = img.copy() # avoid img_copy = img as both images will be modified 
    
    if vertex == True:
        # draw vertices
        for pt in pts:
            cv2.circle(img_copy, pt, rad, col, -thick, lineType=8, shift=0)
    
   
    if edge == True:
        # draw edges
        N = len(pts)
        for i in range(N):
            
            cv2.line(img_copy, pts[ i ], pts[ (i+1) % N ], col, thick)
    
    return img_copy

%matplotlib qt
image = mpimg.imread(test_images[2])
image = cal_undistort(image, objpoints, imgpoints)
img_size = ( image.shape[1], image.shape[0] )

# Manually selecting the cordinnates for a src points of RoI
src_p1 = (316,660)  # bottom left
src_p2 = (1050,660) # bottom right
src_p3 = (762,480)  # top right
src_p4 = (578,480)  # top left

src = np.float32([ src_p1, src_p2, src_p3, src_p4 ])

image_RoI = drawConvexPolygonEdgesVertices(image, [ src_p1, src_p2, src_p3, src_p4 ])

plt.imshow(image_RoI)
plt.show()

%matplotlib inline

height = image.shape[0]
width = image.shape[1]
# Manually selecting the cordinnates for a dst points of RoI
# Might need  a more reliable way to select src and dst points

dst_p1 = (300, height - 30)       # bottom left
dst_p2 = (width-330, height - 30) # bottom right
dst_p3 = (width - 250, 250)         # top right
dst_p4 = (300+70, 250)               # top left

dst = np.float32([ dst_p1, dst_p2, dst_p3, dst_p4])

# generate perspective transofrmation matrix given source and destination points
M = cv2.getPerspectiveTransform(src, dst)

# perform perspective transformation
warped = cv2.warpPerspective(image, M, img_size, flags=cv2.INTER_LINEAR)

warped_RoI = drawConvexPolygonEdgesVertices(warped, [ dst_p1, dst_p2, dst_p3, dst_p4])

PlotComparisonView(image_RoI, warped_RoI, 'Perspective transformed ', colormap = (None, None))

# Check result for all test images
for i in range ( len(test_images) ):
    
    img = mpimg.imread(test_images[i])
    undistort_img = cal_undistort(img, objpoints, imgpoints)

    warped_img = cv2.warpPerspective(undistort_img, M, img_size, flags=cv2.INTER_LINEAR)

    PlotComparisonView(undistort_img, warped_img, 'Perspective transformed ', (None, 'gray'))

# Check perspective transfom result for all test binary images
test_warped_imgs = []
for i in range ( len(test_images) ):
    
    img = mpimg.imread(test_images[i])
    undistort_img = cal_undistort(img, objpoints, imgpoints)

    
    s_binary = to_HLS_Color_Channel(img)
    
    combined_threshold = combinedGradient_thresholdedBinary(s_binary)
    
    warped_img = cv2.warpPerspective(combined_threshold, M, img_size, flags=cv2.INTER_LINEAR)
    test_warped_imgs += [warped_img]

    PlotComparisonView(combined_threshold, warped_img, 'Perspective transformed ', ('gray', 'gray'))

Detect lane pixels and fit to find the lane boundary.¶

# reuse histogram from course material
def hist(img):
    # TO-DO: Grab only the bottom half of the image
    # Lane lines are likely to be mostly vertical nearest to the car

    height = img.shape[0]
    bottom_half = img[ height//2:,: ]

    # TO-DO: Sum across image pixels vertically - make sure to set `axis`
    # i.e. the highest areas of vertical lines should be larger values
    histogram = np.sum( bottom_half, axis=0)
    
    return histogram

for i in range(6):
    # Create histogram of image binary activations
    histogram = hist(test_warped_imgs[i])

    # Visualize the resulting histogram
    fig = plt.figure(1, figsize=(10, 4))
    plt.title("Random title")
    plt.grid()
    plt.plot(histogram)
    plt.show()

I'll detect now lane pixels on the bird's eye view using sliding windows approach and fit 2nd order polynomial for the lane.¶

def detectLanePixels(test_im, stats=True):
    
    # HYPERPARAMETERS
    # Choose the number of sliding windows
    nwindows = 9
    # Set the width of the windows +/- margin
    margin = 100
    # Set minimum number of pixels found to recenter window
    minpix = 50
    
    # Make sure image values are in [0, 255] interval
    if np.max(test_im) == 1:
        test_im = np.uint8(255*test_im)
    
    # Create a histogram for the second half of the image
    histogram = hist(test_im)

    # Create output image to draw on and visualize result
    out_img = np.dstack((test_im, test_im, test_im))

    # peak of left and right part of the histogram are starting points of left and right lanes
    midpoint = np.int(histogram.shape[0]//2)
    leftxbase = np.argmax(histogram[:midpoint])
    rightxbase = np.argmax(histogram[midpoint:]) + midpoint


    # Set height of windows - based on nwindows above and image shape
    window_height = np.int(test_im.shape[0]//nwindows)

    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = test_im.nonzero() # nonzero is a 2-elements tuple
    nonzeroy = np.array(nonzero[0]) # first element is an array of y coordinates of non-zero pixels
    nonzerox = np.array(nonzero[1]) # second element is an array of x coordinates of non-zero pixels

    # Current positions to be updated later for each window in nwindows
    leftx_current = leftxbase
    rightx_current = rightxbase

    # Create empty lists to receive left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []
    
    # This list is only for a general stat purpose
    activatedPixels_perWindow = []

    # Step through the windows one by one
    for window in range(nwindows):
        # Identify window boundaries in x and y (and right and left)
        win_y_low = test_im.shape[0] - (window+1)*window_height
        win_y_high = test_im.shape[0] - window*window_height

        #Find the four boundaries of the left & right windows on x axis 
        win_xleft_low = leftx_current - margin  
        win_xleft_high = leftx_current + margin 
        win_xright_low = rightx_current - margin  
        win_xright_high = rightx_current + margin

        #Identify the nonzero pixels in x and y within the window
        left_nonzero_pixels = ( (nonzerox > win_xleft_low) & (nonzerox < win_xleft_high)
                               & (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]

        right_nonzero_pixels = ( (nonzerox > win_xright_low) & (nonzerox < win_xright_high)
                               & (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]

        #print(len(left_nonzero_pixels), len(right_nonzero_pixels)) # How many non zero pixels indices are on 
                                                                   # the left & right line in current window
        activatedPixels_perWindow.insert(0,[len(left_nonzero_pixels), len(right_nonzero_pixels)])    
        
        # add the result to the one from previous windows
        left_lane_inds.append(left_nonzero_pixels) 
        right_lane_inds.append(right_nonzero_pixels) 

        # Draw the windows on the visualization image
        cv2.rectangle(out_img,(win_xleft_low,win_y_low), (win_xleft_high,win_y_high),(0,255,0), 5) 
        cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 5)

        # recalculating the base x-coordinates of left and right windows
        if (len(left_nonzero_pixels) > minpix):
            # Position of activated pixels in current left window
            leftx_pixelpos_current = nonzerox[left_nonzero_pixels]
            leftx_current = np.int(np.mean( leftx_pixelpos_current, axis=0) )
            #print("new left base is ==>", leftx_current)

        if ( len( right_nonzero_pixels ) > minpix):
            # Position of activated pixels in current right window
            rightx_pixelpos_current = nonzerox[right_nonzero_pixels]
            rightx_current = np.int(np.mean( rightx_pixelpos_current, axis=0) )
            #print("new right base is ==>", rightx_current)

    # reorganise indices across all windows
    left_lane_inds = np.concatenate( left_lane_inds )
    right_lane_inds = np.concatenate( right_lane_inds )
    
    if stats == True:
        print("leftxbase : ", leftxbase, 'rightxbase : ', rightxbase)
        print("Number of non zero pixels are in the whole image", nonzero[0].shape) #how many non zero pixels are in the whole image
        print("Number of non zero pixels indices on the left lane", left_lane_inds.shape) # how many non zero pixels indices are on the left line
        print("Number of non zero pixels indices on the right lane", right_lane_inds.shape) # how many non zero pixels indices are on the right line
        print("Number of activated pixels per window", np.array(activatedPixels_perWindow) )

    # get pixel position from pixel indices
    leftx_pos = nonzerox[left_lane_inds]
    lefty_pos = nonzeroy[left_lane_inds]
    rightx_pos = nonzerox[right_lane_inds]
    righty_pos = nonzeroy[right_lane_inds]

    out_img[ lefty_pos, leftx_pos] = [255, 0, 0]
    out_img[ righty_pos, rightx_pos] = [0, 0, 255]
    
    return leftx_pos, lefty_pos, rightx_pos, righty_pos, out_img

# Apply on all test images
for i in range(5):
    test_img = test_warped_imgs[i]

    lane_pixel_img = detectLanePixels(test_img, stats=False)[4]

    plt.imshow(lane_pixel_img)
    plt.show()

Let's fit a second order polymomial into the previous results:¶

def fit_polynomial_toLane(image, xm_ym_per_pix = (3.7/700, 30/720), plot_poly=True):
    
    # fit a polynomial into the detected pixels so far
    test_img = test_warped_imgs[5]
    leftx, lefty, rightx, righty, lanePixel_im = detectLanePixels(image, stats=False)

    # fitting into a 2nd order polynomial x = f(y)
    #leftfit = np.polyfit( lefty*xm_ym_per_pix[1], leftx*xm_ym_per_pix[0], 2 )
    #rightfit = np.polyfit( righty*xm_ym_per_pix[1], rightx*xm_ym_per_pix[0], 2)

    leftfit = np.polyfit( lefty, leftx, 2 )
    rightfit = np.polyfit( righty, rightx, 2)
    #print("leftfit", leftfit, "rightfit", rightfit)
    # Use the fitting parameters to plot the polynomial on the image
    ## generate y axis base 
    ploty = np.linspace(0, image.shape[0]-1, image.shape[0]).astype(int)
    #ploty = ploty*xm_ym_per_pix[1]
    
    ## compute leftx = f_left(y) & rightx = f_right(y)
    plot_leftx = ( leftfit[0]*ploty**2 + leftfit[1]*ploty + leftfit[2] ).astype(int)
    plot_rightx = ( rightfit[0]*ploty**2 + rightfit[1]*ploty + rightfit[2] ).astype(int)
    
    # plot left and right fitting on lanePixel_im
    #if plot_poly == True:
    #plt.plot( plot_leftx, ploty, color='yellow' )
    #plt.plot( plot_rightx, ploty, color='yellow')
    
    #print(ploty)
    #print(plot_rightx)
    for paint in range(-5,5):
        try:
            lanePixel_im[ ploty, plot_leftx+paint] = [255, 0, 0] # will be painting only a single pixel per y value => not very visible
            lanePixel_im[ ploty, plot_rightx+paint] = [255, 0, 0]
        except:
            break
    #return leftfit, rightfit, lanePixel_im
    return leftfit, rightfit, lanePixel_im

# Let's run on all 6 images
for i in range(6):
    
    current_test_img = test_warped_imgs[i]

    current_out_img = fit_polynomial_toLane(current_test_img)[2]
        
    plt.imshow(current_out_img)
    #cv2.imwrite("output_images/polyfit"+str(i)+".jpg", current_out_img)
    plt.show()

Compute curvature of the fitted polynomial¶

def compute_laneCurvature_vehiculeOffset(test_im):
    
    # introducing the conversion from pixel world to real scale meter world
    xm_per_pix = 3.7/700 # a rigourous reason should be given here?
    ym_per_pix = 30/720
    width = image.shape[1]

    left_fit, right_fit, polyfit_im = fit_polynomial_toLane(test_im, (xm_per_pix,ym_per_pix), plot_poly=True)
    plt.imshow(polyfit_im)

    ploty = np.linspace(0, test_im.shape[0]-1, test_im.shape[0])

    # Select the point where the curvature is calculated
    y_eval = np.max(ploty)

    a_l = left_fit[0]
    b_l = left_fit[1]
    a_r = right_fit[0]
    b_r = right_fit[1]

    # compute left and right curvature using formula from class material
    left_curverad = np.power(1 + (2*left_fit[0]*y_eval+left_fit[1])**2, 3/2)/abs(2*left_fit[0])  
    right_curverad = np.power(1 + (2*right_fit[0]*y_eval+right_fit[1])**2, 3/2)/abs(2*right_fit[0]) 

    print("left_lane_curvature", left_curverad, "right_lane_curvature", right_curverad)
    
    ########## Vehicle position ###########
    
    ## compute leftx = f_left(y) & rightx = f_right(y)
    plot_leftx = ( left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2] ).astype(int)
    plot_rightx = ( right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2] ).astype(int)
    
    vehicle_pos = ( plot_rightx[-1] +  plot_leftx[-1] )//2 # midpoint of the two lines base

    vehicle_offset = xm_per_pix*( vehicle_pos - width//2 )

    #print("Vehicle position -->", vehicle_pos)

    print("Vehicle offset -->", vehicle_offset)

    return left_curverad, right_curverad, vehicle_offset

test_img = test_warped_imgs[1]
print( compute_laneCurvature_vehiculeOffset( test_img ) )

left_lane_curvature 2399.839279967828 right_lane_curvature 3635.939618807656
Vehicle offset --> 0.1162857142857143
(2399.839279967828, 3635.939618807656, 0.1162857142857143)

Project images back into original perspective¶

# perspective transform inverse

# generate perspective transofrmation matrix given source and destination points
Minv = cv2.getPerspectiveTransform(dst, src)

warped_imgs = [] # inversed perspective images

for i in range (6):
    test_img = test_warped_imgs[i]
    lane_pixel_img = fit_polynomial_toLane(test_img)[2]

    # perform perspective transformation
    warped = cv2.warpPerspective(lane_pixel_img, Minv, img_size, flags=cv2.INTER_LINEAR)
    warped_imgs += [warped]
    
    PlotComparisonView(lane_pixel_img, warped, 'inverse perspective', colormap = (None, None))

    #plt.imshow(warped)
    #plt.show()

Draw sliding windows result on initial input images¶

combined_imgs = []

for i in range(5):
    
    warped = warped_imgs[i]

    img = mpimg.imread(test_images[i])

    combined_img = warped | img
    combined_imgs += [combined_img]

    PlotComparisonView(warped, combined_img, 'Final perspective', colormap = (None, None))
    #cv2.imwrite("output_images/finalPerspective"+str(i)+".jpg", cv2.cvtColor(combined_img, cv2.COLOR_RGB2BGR))

The Lane Finding full pipeline for still images:¶

def advancedLaneFindingPipeline(input_img):
    
    Pipeline = []
    
    # undistort input image
    undistort_img = cal_undistort(input_img, objpoints, imgpoints)
    Pipeline += [undistort_img]

    # create a binary from HLS color space
    hls_binary = to_HLS_Color_Channel(undistort_img)
    Pipeline += [hls_binary]
    
    # create a binary using axis, magnitude and direction gradient on hls_binary
    combined_threshold = combinedGradient_thresholdedBinary(hls_binary)
    Pipeline += [combined_threshold]
    
    # perform perspective transform to bird-eye view
    warped_img = cv2.warpPerspective(combined_threshold, M, img_size, flags=cv2.INTER_LINEAR)
    Pipeline += [warped_img]
    
    # detect lane pixel on bird-eye view image and generate fitting polynomial
    lane_pixel_img = fit_polynomial_toLane(warped_img)[2]
    Pipeline += [lane_pixel_img]
    current_fit = fit_polynomial_toLane(warped_img)[:2]
    print(current_fit)

    # perform inverse perspective transformation from bird-eye view
    unwarped_img = cv2.warpPerspective(lane_pixel_img, Minv, img_size, flags=cv2.INTER_LINEAR)
    Pipeline += [unwarped_img]
    
    # draw result on input_img
    finalresult_img = unwarped_img | undistort_img
    Pipeline += [finalresult_img]
    
    #PlotComparisonView(input_img, finalresult_img, 'Final perspective', colormap = (None, None))
    
    # save result into output_images folder
    #cv2.imwrite("output_images/finalPerspective"+str(i)+".jpg", cv2.cvtColor(combined_img, cv2.COLOR_RGB2BGR))
    
    
    # save result into output_images folder
    #for i in range( len(Pipeline)):
    #    if 1<= i <=3:
    #        cv2.imwrite("output_images/SingleImgs"+str(i)+".jpg", 255*Pipeline[i])
    #    else:
    #        cv2.imwrite("output_images/SingleImgs"+str(i)+".jpg", cv2.cvtColor(Pipeline[i], cv2.COLOR_RGB2BGR))

    
    return finalresult_img

Running full pipeline on test images¶

for i in range(6):    
    my_img = mpimg.imread(test_images[i])
    final = advancedLaneFindingPipeline(my_img)

(array([ 1.44483407e-04, -1.85988807e-01,  3.67378032e+02]), array([ 7.86749197e-05, -2.05128931e-01,  1.05672169e+03]))
(array([-2.07508988e-04,  2.89371359e-01,  2.37119575e+02]), array([-3.85815184e-05,  1.83403975e-01,  8.90526832e+02]))
(array([ 6.18629678e-05, -2.22803581e-01,  4.22955832e+02]), array([ 1.27099636e-04, -2.64287294e-01,  1.06585834e+03]))
(array([ 6.86260490e-05, -1.16409990e-01,  3.58811198e+02]), array([ 4.40502225e-04, -4.17506412e-01,  1.07814802e+03]))
(array([ 3.75879036e-04, -4.24751961e-01,  3.72873739e+02]), array([ 8.86968426e-05, -2.10078059e-01,  1.04409158e+03]))
(array([ 1.27110106e-04, -2.87349842e-01,  4.54707032e+02]), array([ 2.92852926e-04, -3.57884806e-01,  1.10379368e+03]))

Video Pipeline¶

First, Let's try this pipeline on the initial video stream as naive "independant" images¶

# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML

def process_image(image):
    # NOTE: The output you return should be a color image (3 channel) for processing video below
    # TODO: put your pipeline here,
    result = advancedLaneFindingPipeline(image)
    # you should return the final output (image where lines are drawn on lanes)

    return result

video_output = 'output_videos/project_video_notFinal.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
clip1 = VideoFileClip("project_video.mp4").subclip(0, 2)
print(type(clip1))
#clip1 = VideoFileClip("project_video.mp4")

white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(video_output, audio=False)

<class 'moviepy.video.io.VideoFileClip.VideoFileClip'>
(array([-1.88608172e-04,  2.71122790e-01,  2.38338530e+02]), array([-3.49962672e-05,  1.44431976e-01,  9.09043012e+02]))
Moviepy - Building video output_videos/project_video_notFinal.mp4.
Moviepy - Writing video output_videos/project_video_notFinal.mp4

t:   0%|                                                                              | 0/50 [00:00<?, ?it/s, now=None]

(array([-1.88608172e-04,  2.71122790e-01,  2.38338530e+02]), array([-3.49962672e-05,  1.44431976e-01,  9.09043012e+02]))

t:   4%|██▊                                                                   | 2/50 [00:01<00:38,  1.25it/s, now=None]

(array([-1.99051163e-04,  2.80889357e-01,  2.37117560e+02]), array([-9.04143749e-05,  1.87284630e-01,  8.99178790e+02]))

t:   6%|████▏                                                                 | 3/50 [00:03<00:47,  1.01s/it, now=None]

(array([-2.05452211e-04,  2.87983340e-01,  2.36291863e+02]), array([-6.03976561e-05,  1.63922885e-01,  9.00718405e+02]))

t:   8%|█████▌                                                                | 4/50 [00:04<00:56,  1.24s/it, now=None]

(array([-2.24178356e-04,  3.02383742e-01,  2.34636812e+02]), array([-1.70984514e-04,  2.68260741e-01,  8.77393635e+02]))

t:  10%|███████                                                               | 5/50 [00:06<00:59,  1.33s/it, now=None]

HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(video_output))

Let's make some improvements to this naive appraoch¶

The main two points that will be adressed are:

Avoiding blind searching in every new frame: searching for the new lines around the area where the previous ones were found.
Smoothing the detected lines across the frames using a moving average for the last n detected good polynomial fits. In order to adress these points we will need to keep track of some characteristics of each line acroos the different frames. For this reason a Class Line() is defined with the attributes we want to keep track of along the video.

# Define a class to receive the characteristics of each line detection
class Line():
    def __init__(self):
        # was the line detected in the last iteration?
        self.detected = False  
        
        # x values of the last n fits of the line
        self.recent_xfitted = []
        
        #average x values of the fitted line over the last n iterations
        self.bestx = None     
        
        #polynomial coefficients averaged over the last n iterations
        self.best_fit = None  
        
        #polynomial coefficients for the most recent fit
        self.current_fit = [np.array([False])]
        
        #polynomial coefficients for the most recent fits
        self.recent_fits = [np.array([False])]  
        
        #radius of curvature of the line in some units
        self.radius_of_curvature = None 
        
        #radius of curvature Array of the line in some units
        self.radius_of_curvature_array = [np.array([])] 
        
        #distance in meters of vehicle center from the line
        self.line_base_pos = None 

        # tracking number of missed lines in a row
        self.missCounter = 4 # initialized to 4 to be able to use sliding windows in the first run

        
# Create two instances from this class
left_line = Line()
right_line = Line()

Here this function is similar to the one with still images, it does add however an additionnal arguments sliding_windows that decides whether we want to perform a sliding windows search all over again in the full frame, or just search locally around the previous detections.

def detectLanePixels_vid(test_im, sliding_windows, stats=True):
    """
    This function returns the positions of activated pixels in left and right lanes
    and the output image with those pixels painted
    """
    
    # HYPERPARAMETERS
    # Choose the number of sliding windows
    nwindows = 9
    # Set the width of the windows +/- margin
    margin = 100
    # Set minimum number of pixels found to recenter window
    minpix = 50
    
    # Make sure image values are in [0, 255] interval
    if np.max(test_im) == 1:
        test_im = np.uint8(255*test_im)
    
    # Create a histogram for the second half of the image
    histogram = hist(test_im)

    # Create output image to draw on and visualize result
    out_img = np.dstack((test_im, test_im, test_im))

    # peak of left and right part of the histogram are starting points of left and right lanes
    midpoint = np.int(histogram.shape[0]//2)
    leftxbase = np.argmax(histogram[:midpoint])
    rightxbase = np.argmax(histogram[midpoint:]) + midpoint

    # Set height of windows - based on nwindows above and image shape
    window_height = np.int(test_im.shape[0]//nwindows)

    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = test_im.nonzero() # nonzero is a 2-elements tuple
    nonzeroy = np.array(nonzero[0]) # first element is an array of y coordinates of non-zero pixels
    nonzerox = np.array(nonzero[1]) # second element is an array of x coordinates of non-zero pixels

    # Current positions to be updated later for each window in nwindows
    leftx_current = leftxbase
    rightx_current = rightxbase
    
    
    if sliding_windows == True: # use sliding windows approach
        print("--> Using Sliding Windows Approach <--")
        # Create empty lists to receive left and right lane pixel indices
        left_lane_inds = []
        right_lane_inds = []

        # This list is only for a general stat purpose
        activatedPixels_perWindow = []

        # Step through the windows one by one
        for window in range(nwindows):
            # Identify window boundaries in x and y (and right and left)
            win_y_low = test_im.shape[0] - (window+1)*window_height
            win_y_high = test_im.shape[0] - window*window_height

            #Find the four boundaries of the left & right windows on x axis 
            win_xleft_low = leftx_current - margin  
            win_xleft_high = leftx_current + margin 
            win_xright_low = rightx_current - margin  
            win_xright_high = rightx_current + margin

            #Identify the nonzero pixels in x and y within the window
            left_nonzero_pixels = ( (nonzerox > win_xleft_low) & (nonzerox < win_xleft_high)
                                   & (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]

            right_nonzero_pixels = ( (nonzerox > win_xright_low) & (nonzerox < win_xright_high)
                                   & (nonzeroy > win_y_low) & (nonzeroy < win_y_high) ).nonzero()[0]

            #print(len(left_nonzero_pixels), len(right_nonzero_pixels)) # How many non zero pixels indices are on 
                                                                       # the left & right line in current window
            activatedPixels_perWindow.insert(0,[len(left_nonzero_pixels), len(right_nonzero_pixels)])    

            # add the result to the one from previous windows
            left_lane_inds.append(left_nonzero_pixels) 
            right_lane_inds.append(right_nonzero_pixels) 

            # Draw the windows on the visualization image
            cv2.rectangle(out_img,(win_xleft_low,win_y_low), (win_xleft_high,win_y_high),(0,255,0), 5) 
            cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 5)

            # recalculating the base x-coordinates of left and right windows
            if (len(left_nonzero_pixels) > minpix):
                # Position of activated pixels in current left window
                leftx_pixelpos_current = nonzerox[left_nonzero_pixels]
                leftx_current = np.int(np.mean( leftx_pixelpos_current, axis=0) )
                #print("new left base is ==>", leftx_current)

            if ( len( right_nonzero_pixels ) > minpix):
                # Position of activated pixels in current right window
                rightx_pixelpos_current = nonzerox[right_nonzero_pixels]
                rightx_current = np.int(np.mean( rightx_pixelpos_current, axis=0) )
                #print("new right base is ==>", rightx_current)

        # reorganise indices across all windows
        left_lane_inds = np.concatenate( left_lane_inds )
        right_lane_inds = np.concatenate( right_lane_inds )
        
    
    elif sliding_windows == False: # Use info from previous frames
        print("--> Searching Around previous PolyFits Approach <--")

        margin = 100
        
        # Get left_fit and right_fit of last frame lines instances
        last_left_fit = left_line.best_fit
        last_right_fit = right_line.best_fit

        # Create  lists to receive left and right lane pixel indices by exploring the region next to where
        # last lanes polyfit were found
        left_lane_inds = ( (nonzerox > (last_left_fit[0]*(nonzeroy**2) + last_left_fit[1]*nonzeroy + 
                        last_left_fit[2] - margin) ) & (nonzerox < (last_left_fit[0]*(nonzeroy**2) + 
                        last_left_fit[1]*nonzeroy + last_left_fit[2] + margin)))

        right_lane_inds = ((nonzerox > (last_right_fit[0]*(nonzeroy**2) + last_right_fit[1]*nonzeroy + 
                        last_right_fit[2] - margin)) & (nonzerox < (last_right_fit[0]*(nonzeroy**2) + 
                        last_right_fit[1]*nonzeroy + last_right_fit[2] + margin)))


    

    # get pixel position from pixel indices
    leftx_pos = nonzerox[left_lane_inds]
    lefty_pos = nonzeroy[left_lane_inds]
    rightx_pos = nonzerox[right_lane_inds]
    righty_pos = nonzeroy[right_lane_inds]

    out_img[ lefty_pos, leftx_pos] = [255, 0, 0]
    out_img[ righty_pos, rightx_pos] = [0, 0, 255]
    
    if stats == True:
        print("leftxbase : ", leftxbase, 'rightxbase : ', rightxbase)
        print("Number of non zero pixels are in the whole image", nonzero[0].shape) #how many non zero pixels are in the whole image
        print("Number of non zero pixels indices on the left lane", left_lane_inds.shape) # how many non zero pixels indices are on the left line
        print("Number of non zero pixels indices on the right lane", right_lane_inds.shape) # how many non zero pixels indices are on the right line

    return leftx_pos, lefty_pos, rightx_pos, righty_pos, out_img

def fit_polynomial_toLane_vid(image, last_fit, xm_ym_per_pix = (3.7/700, 30/720), plot_poly=True):
    
    # fit a polynomial into the detected pixels so far
    test_img = test_warped_imgs[5]
    leftx, lefty, rightx, righty, lanePixel_im = detectLanePixels_vid(image, last_fit, stats=False)

    # fitting into a 2nd order polynomial x = f(y)
    #leftfit = np.polyfit( lefty*xm_ym_per_pix[1], leftx*xm_ym_per_pix[0], 2 )
    #rightfit = np.polyfit( righty*xm_ym_per_pix[1], rightx*xm_ym_per_pix[0], 2)

    leftfit = np.polyfit( lefty, leftx, 2 )
    rightfit = np.polyfit( righty, rightx, 2)
    #print("leftfit", leftfit, "rightfit", rightfit)
    # Use the fitting parameters to plot the polynomial on the image
    ## generate y axis base
    width = image.shape[1]
    height = image.shape[0]
    
    ploty = np.linspace(0, height-1, height).astype(int)
    #ploty = ploty*xm_ym_per_pix[1]
    
    ## compute leftx = f_left(y) & rightx = f_right(y)
    plot_leftx = ( leftfit[0]*ploty**2 + leftfit[1]*ploty + leftfit[2] ).astype(int)
    plot_rightx = ( rightfit[0]*ploty**2 + rightfit[1]*ploty + rightfit[2] ).astype(int)
    
    # plot left and right fitting on lanePixel_im
    #if plot_poly == True:
    #plt.plot( plot_leftx, ploty, color='yellow' )
    #plt.plot( plot_rightx, ploty, color='yellow')
    
    #print(ploty)
    #print(plot_rightx)
    for paint in range(-5,5):
        try:
            lanePixel_im[ ploty, plot_leftx+paint] = [255, 0, 0] 
            lanePixel_im[ ploty, plot_rightx+paint] = [255, 0, 0]
        except:
            break
    
    ######### Curvature radius ##########  
    
    ## fit poly in real world space
    y_eval = np.max(ploty)
    xm_per_pix = xm_ym_per_pix[0]
    ym_per_pix = xm_ym_per_pix[1]
    
    leftfit_rw = np.polyfit( lefty*ym_per_pix, leftx*xm_per_pix, 2 )
    rightfit_rw = np.polyfit( righty*ym_per_pix, rightx*xm_per_pix, 2) 
    
    # Calculation of R_curve (radius of curvature)
    left_curverad = ((1 + (2*leftfit_rw[0]*y_eval*ym_per_pix + leftfit_rw[1])**2)**1.5) / np.absolute(2*leftfit_rw[0])
    right_curverad = ((1 + (2*rightfit_rw[0]*y_eval*ym_per_pix + rightfit_rw[1])**2)**1.5) / np.absolute(2*rightfit_rw[0])
    
    left_line.radius_of_curvature_array.append(left_curverad)
    right_line.radius_of_curvature_array.append(right_curverad)
    
    print("Curvature radius Real World - Left =>", left_curverad, "m - Right", right_curverad, "m")
    
    ########## Vehicle position ###########
   
    vehicle_pos = ( plot_rightx[-1] +  plot_leftx[-1] )//2 # midpoint of the two lines base
    
    vehicle_offset = xm_per_pix*( vehicle_pos - width//2 )
    
    #print("Vehicle position -->", vehicle_pos)
    
    print("Vehicle offset -->", vehicle_offset)

    ########## Sanity Check ############
    
    sanity = True

    ## Inconsistent distance between the two lanes across the heigth of the road
    LinesDiff = plot_rightx - plot_leftx
    NbPointsPerSegment = 30
    
    top = sum( LinesDiff[ :NbPointsPerSegment ] )
    middle = sum( LinesDiff[ height//2:height//2 + NbPointsPerSegment] )
    bottom = sum( LinesDiff[ -NbPointsPerSegment:] )
    
    print("Top --->", top, "Middle --->", middle, "Bottom --->", bottom )
    
    curvrad_diff = 2000   
    
    if abs( top - middle ) > curvrad_diff or abs( top - bottom ) > curvrad_diff or abs( middle - bottom ) > curvrad_diff:
        
        print( "---> Distance between lines is not consistent <---")
        leftfit = left_line.best_fit
        rightfit = right_line.best_fit
        left_line.detected = False
        right_line.detected = False
        left_line.missCounter += 1
        right_line.missCounter += 1
        sanity = False 
        
    else:
        
        # Sanity Check successful - current lines to be included
        left_line.detected = True
        right_line.detected = True
        left_line.radius_of_curvature = int( left_curverad )
        right_line.radius_of_curvature = int( right_curverad )
        left_line.line_base_pos = vehicle_offset
        
        left_line.recent_fits.append( leftfit )
        right_line.recent_fits.append( rightfit )
        
        ## The approach of computing Average is subject to improvement
        
        n = 3 # number of frames across which we want to compute the average
        if len(left_line.recent_fits) > n and left_line.missCounter <= 3:
            
            left_line.best_fit = np.average(left_line.recent_fits[-n:], axis=0)
            right_line.best_fit = np.average(right_line.recent_fits[-n:], axis=0)
        else:
            
            # don't use the average
            left_line.best_fit = leftfit
            right_line.best_fit = rightfit
            
        sanity = True
        
    return left_line.best_fit, right_line.best_fit, lanePixel_im, sanity

def putTextToFrame(image, curvature, offset):
    #### Write curvature radius and vehicule offset info to final image
    
    # font 
    font = cv2.FONT_HERSHEY_SIMPLEX 
    # fontScale 
    fontScale = 1.5
    # Blue color in BGR 
    color = (230, 0, 0) 
    # Line thickness of 2 px 
    thickness = 2
    # Using cv2.putText() method 
    msg1 = 'Curvature Radius : ' + str(curvature)+' m'
    msg2 = 'Vehicule Offset : ' + str( round(offset, 2) ) + 'm'
    
    image = cv2.putText(image, msg1, (300, 100) , font, fontScale, color, thickness, cv2.LINE_AA)
    image = cv2.putText(image, msg2, (300, 200), font, fontScale, color, thickness, cv2.LINE_AA)
    
    return image

def advancedLaneFindingPipeline_vid(input_img):
    
    Pipeline = []
    
    # undistort input image
    undistort_img = cal_undistort(input_img, objpoints, imgpoints)
    Pipeline += [undistort_img]

    # create a binary from HLS color space
    hls_binary = to_HLS_Color_Channel(undistort_img)
    Pipeline += [hls_binary]
    
    # create a binary using axis, magnitude and direction gradient on hls_binary
    combined_threshold = combinedGradient_thresholdedBinary(hls_binary)
    Pipeline += [combined_threshold]
    
    # perform perspective transform to bird-eye view
    warped_img = cv2.warpPerspective(combined_threshold, M, img_size, flags=cv2.INTER_LINEAR)
    Pipeline += [warped_img]
    
    # Detect lane pixel on bird-eye view image and generate fitting polynomial
    
    ########### Decide between Sliding Windows or Local search ############
    
    missThreshold = 8
    # Reperform a sliding windows search if accumulated a number of misses above threshold
    if left_line.missCounter + right_line.missCounter >= missThreshold:
        
        sliding_windows = True
        left_line.missCounter = 0
        right_line.missCounter = 0
    
    else:
        
        sliding_windows = False
        
    current_left_fit, current_right_fit, lane_pixel_img, sanity = fit_polynomial_toLane_vid(warped_img, sliding_windows, plot_poly=False)
    
    Pipeline += [lane_pixel_img]
    
    ############ Drawing Fill Poly Region into input frame ################
    
    ## Used code from course material - note: might want to add it as a separate helper function for future use
    
    # Create an image to draw the lines on
    warp_zero = np.zeros_like(warped_img).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    
    # Recast the x and y points into usable format for cv2.fillPoly()
    # These can be added as attributes hence directly accessible ?   
    ploty = np.linspace(0, image.shape[0]-1, image.shape[0])
    left_fitx = ( current_left_fit[0]*ploty**2 + current_left_fit[1]*ploty + current_left_fit[2] ).astype(int)
    right_fitx = ( current_right_fit[0]*ploty**2 + current_right_fit[1]*ploty + current_right_fit[2] ).astype(int)
    
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
        
    # Warp the blank back to original image space using inverse perspective matrix (Minv)
    newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0])) 
    
    # Combine the result with the original image
    finalresult_img = cv2.addWeighted(input_img, 1, newwarp, 0.3, 0)
    
    finalresult_img = putTextToFrame(finalresult_img, left_line.radius_of_curvature, left_line.line_base_pos )
    
    Pipeline += [finalresult_img]
    


    #PlotComparisonView(lane_pixel_img, finalresult_img, 'Final perspective', colormap = (None, None))
    
    ############ Save Pipeline stages results (just for the writeup ) ################

    # save result into output_images folder
    #for i in range( len(Pipeline)):        
    #    if 1<= i <=3:
    #        cv2.imwrite("output_images/finalResultImgs"+str(i+61)+".jpg", 255*Pipeline[i])          
    #    else:
    #        cv2.imwrite("output_images/finalResultImgs"+str(i+61)+".jpg", cv2.cvtColor(Pipeline[i], cv2.COLOR_RGB2BGR))
            
    return finalresult_img

def process_image(image):
    # NOTE: The output you return should be a color image (3 channel) for processing video below
    # TODO: put your pipeline here,
    result = advancedLaneFindingPipeline_vid(image)
    # you should return the final output (image where lines are drawn on lanes)

    return result

video_output = 'output_videos/project_video_final2.mp4'
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
clip1 = VideoFileClip("project_video.mp4").subclip(0, 2)
#clip1 = VideoFileClip("project_video.mp4")

left_line = Line()
right_line = Line()

white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(video_output, audio=False)

--> Using Sliding Windows Approach <--
Curvature radius Real World - Left => 870.7296470594531 m - Right 4693.69592320204 m
Vehicle offset --> 0.12685714285714286
Top ---> 20069 Middle ---> 19342 Bottom ---> 19738
Moviepy - Building video output_videos/project_video_final2.mp4.
Moviepy - Writing video output_videos/project_video_final2.mp4

t:   0%|                                                                              | 0/50 [00:00<?, ?it/s, now=None]

--> Searching Around previous PolyFits Approach <--
Curvature radius Real World - Left => 869.5606419546223 m - Right 4927.818359817006 m
Vehicle offset --> 0.12685714285714286
Top ---> 20069 Middle ---> 19341 Bottom ---> 19742

t:   4%|██▊                                                                   | 2/50 [00:01<00:43,  1.11it/s, now=None]

--> Searching Around previous PolyFits Approach <--
Curvature radius Real World - Left => 825.2191172619653 m - Right 1869.0707404330333 m
Vehicle offset --> 0.111
Top ---> 19831 Middle ---> 19266 Bottom ---> 19496

t:   6%|████▏                                                                 | 3/50 [00:03<00:52,  1.11s/it, now=None]

--> Searching Around previous PolyFits Approach <--
Curvature radius Real World - Left => 799.3279408594502 m - Right 2682.4919768384575 m
Vehicle offset --> 0.1162857142857143
Top ---> 19876 Middle ---> 19152 Bottom ---> 19463

t:   8%|█████▌                                                                | 4/50 [00:04<00:57,  1.25s/it, now=None]

--> Searching Around previous PolyFits Approach <--
Curvature radius Real World - Left => 732.839528899932 m - Right 866.0716167228234 m
Vehicle offset --> 0.09514285714285714
Top ---> 19175 Middle ---> 19137 Bottom ---> 19333

t:  10%|███████                                                               | 5/50 [00:06<01:00,  1.33s/it, now=None]

HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(video_output))